import re

# 指定要讀取的文本文件名
file_name = '/home/JSDC/017254/code/gitee/map_learing/01_binocular_vision_xuce/output.txt'

# 用於存儲每幀數據的列表
formatted_frames = []

# 從文件中讀取數據
with open(file_name, 'r') as file:
    lines = file.readlines()

for line in lines:
    line = line.strip()  # 去除行首尾空白字符
    if line.startswith("fusepose:"):
        # 解析 fusepose 數據
        fusepose_data = re.findall(r"[-+]?\d*\.\d+|[-+]?\d+", line)
        lidar_time_stamp = int(fusepose_data[0])
        fusepose = (float(fusepose_data[1]), float(fusepose_data[2]), float(fusepose_data[3]))
        rpy = (float(fusepose_data[4]), float(fusepose_data[5]), float(fusepose_data[6]))
    
    elif line.startswith("ai_time:"):
        # 解析 ai_time 數據
        ai_time_data = re.findall(r"\d+", line)
        ai_time_stamp = int(ai_time_data[0])
    
    elif line.startswith("ai_obts:"):
        # 解析 ai_obts 數據
        ai_obts_data = line[len("ai_obts:"):].strip()
        xyz = []
        if ai_obts_data:
            # 將字符串轉換為元組
            ai_obts_tuples = re.findall(r"\((.*?)\)", ai_obts_data)
            xyz = [tuple(map(float, t.split(','))) for t in ai_obts_tuples]

        # 計算 pcl_no
        pcl_no = len(xyz)
        
        # 計算 diff
        diff = ai_time_stamp - lidar_time_stamp
        
        # 將格式化數據添加到列表
        formatted_frame = {
            'lidar_time_stamp': lidar_time_stamp,
            'ai_time_stamp': ai_time_stamp,
            'fusepose': fusepose,
            'rpy': rpy,
            'xyz': xyz,
            'diff': diff,
            'pcl_no': pcl_no
        }
        formatted_frames.append(formatted_frame)

# 輸出結果
for frame in formatted_frames:
    print(frame)
